from Rosmaster_Lib import Rosmaster  #ROS驱动器底层驱动库
import time
import matplotlib.pyplot as plt  #画图库
import math
import pygame #
import numpy as np 


import xlwt
import xlrd
#使用xlwt库进行保存数据


pygame.init()
pygame.joystick.init()
joystick = pygame.joystick.Joystick(0)



wb = xlwt.Workbook()
# 打开工作区
ws = wb.add_sheet('test')
joystick.init()
midtheta1=92
midtheta2=92
midtheta3=112
midtheta4=90
bot = Rosmaster()
bot.create_receive_threading()
def limittheta(theta,limitnum):
    if theta>=limitnum:
        theta=limitnum
    elif theta<=-limitnum:
        theta=-limitnum
    else : theta=theta
    return theta 

def limitpwm(pwm):
    if pwm>=50:
        pwm=50
    elif pwm<=-50:
        pwm=-50
    else : pwm=pwm
    return pwm 

#  控制PWM  
def pwm_servo(S1, S2, S3, S4):
    
    bot.set_pwm_servo(1, S1)
    bot.set_pwm_servo(2, S2)
    bot.set_pwm_servo(3, S3)
    bot.set_pwm_servo(4, S4)
    #双阿克曼转向
def danAKM(thetaf):
    thetar=-thetaf
   
    thetarl=0
    thetarr=0
    ##??????????? 
    get_thetafl=midtheta1+limittheta(thetaf,20)
    get_thetafr=midtheta2+limittheta(thetaf,20)
    get_thetarl=midtheta3+limittheta(thetarl,20)
    get_thetarr=midtheta4+limittheta(thetarr,20)
    pwm_servo(get_thetafl,get_thetafr,get_thetarl,get_thetarr)



#help(bot)
timess=0
mmlost=0
mmnew=0
mmcha=0
yylost=0
yynew=0
yycha=0
xxx=0
yyy=0
DONE=True
stop=0
joyx=0
vv=0
qianpwm=0
houpwm=0
encoder=[]
hangxiangjiaoall=[]
num=0
cishuo=[]
ws.write(0,0,"yaw")
ws.write(0,1,"xxx")
ws.write(0,2,"yyy")
ws.write(0,3,"mm")
ws.write(0,4,"joyx")
while DONE:
    try:
        num=num+1
        cishuo.append(num)
        ##手柄出发事件
        for event_ in pygame.event.get():
            if event_.type == pygame.JOYBUTTONDOWN or event_.type == pygame.JOYBUTTONUP:
                if joystick.get_button(3)==1:

                    print("x")
                    stop=1
                else :stop=0
                if joystick.get_button(0)==1: 
                    print("A")
                    DONE=False    #按B退出
            elif event_.type == pygame.JOYAXISMOTION:
                    qianpwm=joystick.get_axis(4)
                    houpwm=joystick.get_axis(5)
                    joyx=joystick.get_axis(0)*20
            else:
                qianpwm=0
                houpwm=0
                joyx=0
        if stop==1:
            bot.set_motor(0, 0, 0, 0) #停车
        vv=(qianpwm-houpwm)*5
        bot.set_motor(int(vv),int(vv),int(vv),int(vv))

        print()

        yylost=yynew
        gx,gy,gz=bot.get_gyroscope_data()
        ax,ay,az=bot.get_accelerometer_data()
        mx,my,mz=bot.get_magnetometer_data()
        roll,pitch, yaw =bot.get_imu_attitude_data(True)

        yynew=yaw
        yycha=yynew-yylost
        #print("roll,pitch, yaw",roll,pitch, yaw)
        # print(gx,gy,gz)
        # print(ax,ay,az)
        # print(mx,my,mz)
        #print(roll,pitch,yaw)


        danAKM(joyx)
        mmlost=mmnew
        timess=timess+1
        m1, m2, m3, m4 = bot.get_motor_encoder()  #获取编码器数据
        print("encoder:", m1, m2, m3, m4)    
        mm=(m1+m2+m3+m4)*0.2526/4
        mmnew=mm
        mmcha=mmnew-mmlost

        if timess==1:
            mmcha=0
            yycha=0
        # print("mmcha:", mmcha)
        # print("yycha:", yycha)
        xxx=xxx+mmcha*math.sin(math.radians(-yycha))
        yyy=yyy+mmcha*math.cos(math.radians(-yycha))
        print("xxx:", xxx)
        print("yyy:", yyy)

        hangxiangjiaoall.append(yaw)  #记录航向角数据
        
        time.sleep(.1)#延时0.1s
      
       ##数据读写
        ws.write(num,0,yaw)
        ws.write(num,1,xxx)
        ws.write(num,2,yyy)
        ws.write(num,3,mm)
        ws.write(num,4,joyx)
    except KeyboardInterrupt:
        break
                

del bot
wb.save('/home/sunrise/Desktop/python_test/test10.xls')  #保存数据到指定文件
print("ok!")
